#pragma once

#ifndef M_PI
#define M_PI 3.14159265358979323846  // pi
#endif

/**
 * @brief Clamp value inside a given range
 * @tparam T working type
 * @param val Value to clamp
 * @param min Lower bound of clamping range
 * @param max Upper bound od clamping range
 * @return clamped value
 * @note Assuming range form a valid range (ie: min <= max )
 */
template <typename T>
inline T clamp(const T& val, const T& min, const T& max) {
  return std::max(min, std::min(val, max));
}

/**
 * @brief Convert an angle from degree to radian
 * @param degree Angle in degree
 * @return Same angle in radian
 * @note Assuming input angle is in range [0;360]
 */
inline double D2R(double degree) { return degree * M_PI / 180.0; }

/**
 * @brief Convert an angle from radian to degree
 * @param radian Angle in radian
 * @return Same angle in degree
 * @note Assuming input angle in range [0;2Pi]
 */
inline double R2D(double radian) { return radian / M_PI * 180.0; }

/**
 * @brief skewSymmetric
 * @param radian Angle in radian
 * @return Same angle in degree
 * @note Assuming input angle in range [0;2Pi]
 */
template <typename Derived>
static Eigen::Matrix<typename Derived::Scalar, 3, 3> skewSymmetric(
    const Eigen::MatrixBase<Derived>& q) {
  Eigen::Matrix<typename Derived::Scalar, 3, 3> ans;
  ans << typename Derived::Scalar(0), -q(2), q(1), q(2),
      typename Derived::Scalar(0), -q(0), -q(1), q(0),
      typename Derived::Scalar(0);
  return ans;
}
